/*
 * Copyright (c) 2021, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "ti_msp_dl_config.h"
#include "Encoder.h"
#include "pid.h"
#include "MPU6050.h"
#include "OLED.h"
#include "Sensor.h"


int8_t status = 1;//模式状态机，默认为模式0
int8_t end_status = 0;//监测有无执行上片代码
int16_t MA, MB;//电机转速
int16_t tarMA, tarMB;//PID目标值
int16_t AccX, AccY, AccZ, GyroX, GyroY, GyroZ;//加速度计和陀螺仪数据
Attitude attitude = {0}; 



int main(void)
{
    SYSCFG_DL_init();
    MPU6050_Init();
    OLED_Init();
    NVIC_EnableIRQ(Encoder_INT_IRQN);//GPIO中断打开
    NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN);//TIM中断打开

    while(1)
    {
        if(status == 1 & end_status != 1)
        {
            tarMA = tarMB = 50;
            while (Sensor_mul_status() == 0) //任一GPIO为0
            {
                tarMA = tarMB = 0;
                end_status = 1;
            }
        }
        if(status == 2)
        {
            //先直线
            if(end_status != 2)
            {
                tarMA = tarMB = 50;
                while(Sensor_mul_status() == 0);
                tarMA = tarMB = 0;
                for(volatile int j = 0; j < 5000; j++);
                end_status = 2;//第一次进入巡线
            }

            //读循迹状态
            if(DL_GPIO_readPins(Sensor_PORT, Sensor_PIN_0_PIN)? 0: 1 )
            {
                tarMB +=5;
            }//1
            if(DL_GPIO_readPins(Sensor_PORT, Sensor_PIN_1_PIN)? 0: 1 )
            {
                tarMB +=3;
            }//2
            if(DL_GPIO_readPins(Sensor_PORT, Sensor_PIN_2_PIN)? 0: 1 )
            {
                tarMB +=1;
            }//3
            if(DL_GPIO_readPins(Sensor_PORT, Sensor_PIN_3_PIN)? 0: 1 )
            {
                tarMA = tarMB = 50;
            }//4
            if(DL_GPIO_readPins(Sensor_PORT, Sensor_PIN_4_PIN)? 0: 1 )
            {
                tarMA = tarMB = 50;
            }//5
            if(DL_GPIO_readPins(Sensor_PORT, Sensor_PIN_5_PIN)? 0: 1 )
            {
                tarMA +=1;
            }//6
            if(DL_GPIO_readPins(Sensor_PORT, Sensor_PIN_6_PIN)? 0: 1 )
            {
                tarMA +=3;
            }//7
            if(DL_GPIO_readPins(Sensor_PORT, Sensor_PIN_6_PIN)? 0: 1 )
            {
                tarMA +=5;
            }//8
            if(Sensor_mul_status() == 1)
            {
                tarMA = tarMB = 0;
            }
        }

    }



}









//按键扫描函数
uint8_t KEY_SCAN(void)
{
    static uint8_t key_state = 0;
    static uint8_t key_count = 0;
    uint8_t current_state = DL_GPIO_readPins(KEY_PORT, KEY_control_PIN) == 0 ? 1 : 0;
    
    // 状态一致时计数增加
    if(current_state == key_state) {
        key_count++;
        if(key_count >= 4) {  // 连续4次状态一致才确认
            key_count = 0;
            return current_state;
        }
    } else {  // 状态不一致时重置
        key_state = current_state;
        key_count = 0;
    }
    
    return 0xFF;  // 返回0xFF表示按键状态不稳定
}
//按键扫描受托函数
//20ms扫描一次
void KEY_tick(void)
{
    static uint8_t time = 0;
    static uint8_t curr_status = 0;
    static uint8_t last_status = 0;
    time++;
    if(time == 20)  // 降低扫描间隔，提高响应速度
    {
        last_status = curr_status;
        uint8_t scan_result = KEY_SCAN();
        
        // 只有当返回值有效时才更新状态
        if(scan_result != 0xFF) {
            curr_status = scan_result;
            
            // 检测按键按下（下降沿）
            if(curr_status == 1 && last_status == 0)
            {
                status++;
                if(status > 4)
                {
                    status = 1;
                }
            }
        }
        time = 0;
    }
}
//pid控制受托函数
//100ms计算一次
//如需修改频率，需要进入内部修改公式
void Encoder_TimCaculate(void)
{
    static int16_t TimCaculate ;
    TimCaculate ++;
    if(TimCaculate == 100)
    {
        PID_SetTarget(1,tarMA);
        PID_SetTarget(2,tarMB);
        PID_Control(&MA,&MB);
        TimCaculate = 0;
    }
}

//MPU6050受托函数
//1ms计算一次
void MPU6050_TIM(void)
{
    // 读取传感器数据
    MPU6050_GetData(&AccX, &AccY, &AccZ, &GyroX, &GyroY, &GyroZ);
    // 更新姿态
    UpdateAttitude(AccX, AccY, AccZ, GyroX, GyroY, GyroZ, &attitude);  
}

void TIMER_0_INST_IRQHandler(void)
{
    KEY_tick();//按键触发
    Encoder_TimCaculate();//PID控制
    MPU6050_TIM();//MPU6050控制
    OLED_ShowNum(1, 1, tarMA, 3);
    OLED_ShowNum(2, 1, tarMB, 3);
    OLED_ShowNum(1, 5, MA, 3);
    OLED_ShowNum(2, 5, MB, 3);

}












//该中断用来采样编码器脉冲
void GROUP1_IRQHandler (void)
{
     // 获取GPIOA中断状态 - 只获取我们关心的引脚
    uint32_t ma_a_status = DL_GPIO_getEnabledInterruptStatus(Encoder_MA_A_PORT, Encoder_MA_A_PIN);
    uint32_t mb_a_status = DL_GPIO_getEnabledInterruptStatus(Encoder_MB_A_PORT, Encoder_MB_A_PIN);
    
    // 处理电机A的A相中断
    if (ma_a_status) {
        // 读取B相状态确定方向
        uint8_t direction = DL_GPIO_readPins(Encoder_MA_B_PORT, Encoder_MA_B_PIN) ? 1 : 0;
        // 更新电机A的计数器
        Encoder_Update(0, direction);
        // 清除中断状态
        DL_GPIO_clearInterruptStatus(GPIOA, Encoder_MA_A_PIN);
    }
    
    // 处理电机B的A相中断
    if (mb_a_status) {
        // 读取B相状态确定方向
        uint8_t direction = DL_GPIO_readPins(Encoder_MB_B_PORT, Encoder_MB_B_PIN) ? 1 : 0;
        // 更新电机B的计数器
        Encoder_Update(1, direction);
        // 清除中断状态
        DL_GPIO_clearInterruptStatus(GPIOA, Encoder_MB_A_PIN);
    }
}

